home *** CD-ROM | disk | FTP | other *** search
/ Resource for Source: C/C++ / Resource for Source - C-C++.iso / codelib9 / v_11_07 / 1107076a < prev    next >
Encoding:
Text File  |  1995-11-01  |  9.3 KB  |  369 lines
  1. /*  ALPHABET.C
  2.  
  3.   Demonstrate the alpha-beta filter with a particular
  4.   data profile, with and without gaussian noise, and
  5.   with different alpha-beta values.
  6.  
  7.   The graphic features here assume the presence of a
  8.   VGA monitor.
  9.  
  10.   This code was written for Borland C++, Version 3.1,
  11.   coded for MS-DOS.
  12. */
  13.  
  14. #include <graphics.h>
  15. #include <stdlib.h>
  16. #include <stdio.h>
  17. #include <conio.h>
  18. #include <alloc.h>
  19. #include <time.h>
  20. #include <math.h>
  21.  
  22. #define PLOT1           120    /* range of x for first
  23.                    part of plot */
  24. #define PLOT2           310    /* range of x for second
  25.                    part of plot */
  26. #define PLOT3           430    /* range of x for third
  27.                    part of plot */
  28. #define PLOTEND         640    /* end of x range */
  29.  
  30.  
  31. #define STEADY_STATE1   120.0  /* first steady state y
  32.                    value */
  33. #define STEADY_STATE2   400.0  /* second steady state y
  34.                    value */
  35. #define STEADY_STATE3    70.0  /* third steady state y
  36.                    value */
  37. #define STEADY_STATEBOT 480.0  /* bottom limit for
  38.                    steady state
  39.                    values */
  40.  
  41. #define ALPHA           0.25   /* arbitrary alpha
  42.                    value */
  43.  
  44. void alpha_beta(double *y,double *y_smoothed,
  45.         double alpha,double beta);
  46. void profile(double *y);
  47. void gauss_noise(double *noise);
  48. void plot_it(double *y);
  49. void message(char *string1,char *string2,
  50.          char *string3);
  51.  
  52. void main(void)
  53. {
  54.   int x,graphdriver=DETECT,graphmode,errorcode;
  55.  
  56.   double *y_pure,*y_noisy,*y_smoothed,*noise;
  57.   double alpha,beta;
  58.  
  59.   /**********************************/
  60.   /* allocate memory for the arrays */
  61.   /**********************************/
  62.  
  63.   if((y_pure =
  64.      (double *)malloc(PLOTEND*sizeof(double)))==
  65.      NULL) {
  66.     printf("\a");
  67.     cprintf("Unable to allocate space for array ");
  68.     cprintf("y_pure[]\n\r");
  69.     exit(1);
  70.   }
  71.   if((y_noisy =
  72.      (double *)malloc(PLOTEND*sizeof(double)))==
  73.      NULL) {
  74.     printf("\a");
  75.     cprintf("Unable to allocate space for array ");
  76.     cprintf("y_noisy[]\n\r");
  77.     exit(1);
  78.   }
  79.   if((y_smoothed =
  80.      (double *)malloc(PLOTEND*sizeof(double)))==
  81.      NULL) {
  82.     printf("\a");
  83.     cprintf("Unable to allocate space for array ");
  84.     cprintf("y_smoothed[]\n\r");
  85.     exit(1);
  86.   }
  87.   if((noise =
  88.     (double *)malloc(PLOTEND*sizeof(double)))==NULL) {
  89.     printf("\a");
  90.     cprintf("Unable to allocate space for array ");
  91.     cprintf("noise[]\n\r");
  92.     exit(1);
  93.   }
  94.  
  95.   /****************************/
  96.   /* initialize graphics mode */
  97.   /****************************/
  98.  
  99.   registerbgidriver(EGAVGA_driver);
  100.   initgraph(&graphdriver,&graphmode,"");
  101.   errorcode=graphresult();
  102.   if(errorcode != grOk) {
  103.     printf("\a");
  104.     cprintf("Graphics error: %s\n\r",
  105.         grapherrormsg(errorcode));
  106.     exit(1);
  107.   }
  108.  
  109.   /*************************************************/
  110.   /* generate the pure and corrupted data profiles */
  111.   /*************************************************/
  112.  
  113.   /* generate the clean profile */
  114.   profile(y_pure);
  115.  
  116.   /* generate the noise */
  117.   gauss_noise(noise);
  118.  
  119.   /* corrupt the clean data with noise */
  120.   for(x=0;x<PLOTEND;++x)
  121.     y_noisy[x] = y_pure[x] + noise[x];
  122.  
  123.   /****************************************/
  124.   /* plot a pure profile with underdamped */
  125.   /* alpha-beta smoothing                 */
  126.   /****************************************/
  127.  
  128.   /* plot the clean profile */
  129.   message("Clean data profile","","");
  130.   plot_it(y_pure);
  131.  
  132.   /* compute and plot the smoothed result
  133.       over the clean profile */
  134.   alpha = ALPHA; /* arbitrary alpha value */
  135.   beta = alpha*alpha / (2.0 - alpha); /* standard,
  136.                       underdamped
  137.                       beta value */
  138.   alpha_beta(y_pure,y_smoothed,alpha,beta);
  139.   message("","with",
  140.       "Underdamped alpha-beta smoothing");
  141.   plot_it(y_smoothed);
  142.  
  143.   /* clear the screen and replot
  144.       the smoothed result */
  145.   cleardevice();
  146.   message("Underdamped alpha-beta smoothing of clean \
  147. profile","","");
  148.   plot_it(y_smoothed);
  149.  
  150.   /*****************************************/
  151.   /* plot a noisy profile with underdamped */
  152.   /* alpha-beta smoothing                  */
  153.   /*****************************************/
  154.  
  155.   /* clear the screen and plot the profile */
  156.   cleardevice();
  157.   message("Noisy profile","","");
  158.   plot_it(y_noisy);
  159.  
  160.   /* compute and plot the smoothed result over the
  161.       noisy profile */
  162.   alpha_beta(y_noisy,y_smoothed,alpha,beta);
  163.   message("","with",
  164.       "Underdamped alpha-beta smoothing");
  165.   plot_it(y_smoothed);
  166.  
  167.   /* clear the screen and replot the
  168.       smoothed result */
  169.   cleardevice();
  170.   message("Underdamped alpha-beta smoothing of noisy \
  171. profile","","");
  172.   plot_it(y_smoothed);
  173.  
  174.   /* plot the pure profile over the estimate based on
  175.       the noisy version */
  176.   message(""," with","Clean data profile");
  177.   plot_it(y_pure);
  178.  
  179.   /********************************************/
  180.   /* plot a pure profile with near-critically */
  181.   /* damped alpha-beta smoothing              */
  182.   /********************************************/
  183.  
  184.   /* clear the screen and plot the clean profile */
  185.   cleardevice();
  186.   message("Clean data profile","","");
  187.   plot_it(y_pure);
  188.  
  189.   /* compute and plot the smoothed result over the
  190.       clean profile */
  191.   alpha = ALPHA; /* arbitrary alpha value */
  192.   beta = 0.8*(2.0 - alpha*alpha -
  193.      2.0*sqrt(1 - alpha*alpha))/(alpha*alpha);
  194.   alpha_beta(y_pure,y_smoothed,alpha,beta);
  195.   message("","with","Near-critically damped \
  196. alpha-beta smoothing");
  197.   plot_it(y_smoothed);
  198.  
  199.   /*********************************************/
  200.   /* plot a noisy profile with near-critically */
  201.   /* damped alpha-beta smoothing               */
  202.   /*********************************************/
  203.  
  204.   /* clear the screen and plot the noisy profile */
  205.   cleardevice();
  206.   message("Noisy profile","","");
  207.   plot_it(y_noisy);
  208.  
  209.   /* compute and plot the smoothed response over the
  210.       noisy profile */
  211.   beta = 0.8*(2.0 - alpha*alpha -
  212.      2.0*sqrt(1 - alpha*alpha))/(alpha*alpha);
  213.   alpha_beta(y_noisy,y_smoothed,alpha,beta);
  214.   message("Noisy profile","with",
  215.       "Near-critically damped alpha-beta \
  216. smoothing");
  217.   plot_it(y_smoothed);
  218.  
  219.   /* clear the screen and replot the smoothed result */
  220.   cleardevice();
  221.   message("Near-critically damped alpha-beta \
  222. smoothing of noisy profile","","");
  223.   plot_it(y_smoothed);
  224.  
  225.   /* plot the pure profile over the estimate based on
  226.       the noisy version */
  227.   message(""," with","Clean data profile");
  228.   plot_it(y_pure);
  229.  
  230.   /* prepare to exit */
  231.   closegraph();
  232. }
  233.  
  234. void alpha_beta(double *y,double *y_smoothed,
  235.         double alpha,double beta)
  236.  
  237. /* demonstrate the alpha-beta algorithm */
  238.  
  239. {
  240.   int i;
  241.  
  242.   double s_est,v_est,s_pred,error;
  243.  
  244.   /* initialize the filter */
  245.   s_est = y[0];
  246.   v_est = 0.0;
  247.  
  248.   /* loop through all of the position measurements */
  249.   for(i=0;i<PLOTEND;++i) {
  250.  
  251.     /***********************************/
  252.     /* the alpha-beta filter algorithm */
  253.     /* (with T absorbed into v)        */
  254.     /***********************************/
  255.  
  256.     /* predict the new position */
  257.     s_pred = s_est + v_est;
  258.  
  259.     /* compute the measurement error */
  260.     error = y[i] - s_pred;
  261.  
  262.     /* estimate the true position and velocity */
  263.     s_est = s_pred + alpha*error;
  264.     v_est = v_est  +  beta*error;
  265.  
  266.     /*****************************/
  267.     /* update the smoothed array */
  268.     /*****************************/
  269.  
  270.     y_smoothed[i] = s_est;
  271.   }
  272. }
  273.  
  274. void profile(double *y)
  275.  
  276. /* generate the profile to be studied */
  277.  
  278. {
  279.   int i;
  280.  
  281.   /* first steady-state portion */
  282.   for(i=0;i<PLOT1;++i) y[i] = STEADY_STATE1;
  283.  
  284.   /* ramp from first steady state level to second
  285.      steady state level */
  286.   for(i=PLOT1;i<PLOT2;++i)
  287.     y[i] = (i-PLOT1) * (STEADY_STATE2-STEADY_STATE1) /
  288.                (PLOT2-PLOT1) + STEADY_STATE1;
  289.  
  290.   /* second steady-state portion */
  291.   for(i=PLOT2;i<PLOT3;++i) y[i] = STEADY_STATE2;
  292.  
  293.   /* step to third steady-state portion */
  294.   for(i=PLOT3;i<PLOTEND;++i) y[i] = STEADY_STATE3;
  295. }
  296.  
  297. void gauss_noise(double *noise)
  298.  
  299. /* generate mean-zero gaussian noise using Box-Muller
  300.    method */
  301.  
  302. {
  303.   int i;
  304.  
  305.   double drand1,drand2,pi,sigma=10.0,mean=0.0;
  306.  
  307.   /* define pi */
  308.   pi = 4.0 * atan(1.0);
  309.  
  310.   /* seed the random number generator */
  311.   randomize();
  312.  
  313.   /* generate gaussian noise using the Box-Muller
  314.      method */
  315.   for(i=0;i<PLOTEND;++i) {
  316.  
  317.     /* compute two double random numbers */
  318.     drand1 = (double)rand()/RAND_MAX;
  319.     drand2 = (double)rand()/RAND_MAX;
  320.  
  321.     /* compute the noise term */
  322.     if(drand1 < 1e-10) drand1 = 1e-10; /* avoid
  323.                        division
  324.                        by zero */
  325.     noise[i] = sqrt(2.0*log(1.0/drand1)) *
  326.            cos(2.0 * pi * drand2) * sigma + mean;
  327.   }
  328. }
  329.  
  330. void plot_it(double *y)
  331.  
  332. /* plot one member of array y for each horizontal
  333.    pixel position */
  334.  
  335. {
  336.   int x,y_old;
  337.  
  338.   /* plot the profile */
  339.   y_old = (int)(y[0]+0.5);
  340.   for(x=0;x<PLOTEND;++x) {
  341.     line(x,y_old,x,(int)(y[x]+0.5));
  342.     y_old = (int)(y[x]+0.5);
  343.   }
  344.  
  345.   /* wait for key press */
  346.   getch();
  347. }
  348.  
  349. void message(char *string1,char *string2,
  350.          char *string3)
  351.  
  352. /* print a message of three lines centered at bottom of
  353.    screen */
  354.  
  355. {
  356.   int midx,maxy;
  357.  
  358.   /* get screen parameters */
  359.   midx=getmaxx()/2;
  360.   maxy=getmaxy();
  361.  
  362.   /* print pieces of message, centered */
  363.   settextjustify(CENTER_TEXT,TOP_TEXT);
  364.   outtextxy(midx,maxy-30,string1);
  365.   outtextxy(midx,maxy-20,string2);
  366.   outtextxy(midx,maxy-10,string3);
  367. }
  368.  
  369.